clc; clear;
A = [0 1 0;-1 -2 1; 0 -1 -2];
B = [0 0; 0 -1; 1 1];
Bf = [0; 0; 1]; % fault matrix
C = [1 0 0; 0 1 0];

poles = [-1 -2 -2]; % desired observer poles

% check the observability of the system
if(rank(obsv(A, C)) == size(A, 1))
    disp('the system is observable!')
else
    disp('the system is not observable!')
end

L = place(A', C', poles)'; % compute the observer gain using pole placement

t0 = 0;
tf = 20;

x0 = [0.1; 0.1; 0.1]; % system initial condition
xhat0 = [0; 0; 0]; % Observer initial condition

[t, x] = ode45(@(t, x) SysObsv(t, x, A, B, Bf, C, L), [t0, tf], [x0; xhat0]);
y = (C*x(:,1:3)')';
yhat = (C*x(:,4:6)')';
Res = y - yhat;

len = size(Res, 1);
ResNorm = zeros(len, 1);
for i = 1:len
    ResNorm(i) = norm(Res(i,1:end));
end

figure(1)
subplot(3,1,1);
plot(t, x(:, 1), 'r', t, x(:, 4), 'k--', 'LineWidth',1.2)
legend('$x_1(t)$','$\hat{x}_1(t)$','Interpreter','latex','FontSize',15)
subplot(3,1,2);
plot(t, x(:, 2), 'r', t, x(:, 5), 'k--', 'LineWidth',1.2)
legend('$x_2(t)$','$\hat{x}_2(t)$','Interpreter','latex','FontSize',15)
subplot(3,1,3);
plot(t, x(:, 3), 'r', t, x(:, 6), 'k--', 'LineWidth',1.2)
legend('$x_3(t)$','$\hat{x}_3(t)$','Interpreter','latex','FontSize',15)
figure(2)
subplot(2,1,1)
plot(t, Res(:,1), 'r', 'LineWidth',1.2)
legend('$r_1(t)$','Interpreter','latex','Fontsize',15)

subplot(2,1,2)
plot(t, Res(:,2), 'k', 'LineWidth',1.2)
legend('$r_2(t)$','Interpreter','latex','Fontsize',15)
xlabel('$t$','Interpreter','latex','FontSize',15)

figure(3)
plot(t, ResNorm, 'k', 'LineWidth',1.2)
legend('$\|r(t)\|$','Interpreter','latex','Fontsize',15)
xlabel('$t$','Interpreter','latex','FontSize',15)

function dotx = SysObsv(t,x,A,B,Bf,C,L)
   dotx = zeros(6,1);
%    f = 0;
   if t<5
       f = 0;
   else 
       f = 1;
   end
   y = C*x(1:3);
   yhat = C*x(4:6);
   u = [0; 0];
   dotx(1:3) = A*x(1:3) + B*u+ Bf*f;
   dotx(4:6) = A*x(4:6) + B*u+ L*(y-yhat);
end
